#include <math.h>
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>

#include "Beard.h"
#include "include.h"
#include "GL/glut.h"

Beard::Beard(Vector2D * anchor, int numSegments, float mass, float length, float stringConstant, TextureManager * textureManager, char * textureName)
: Rope(anchor, numSegments, mass, length, stringConstant)
{
	red = 1.0;
	green = 1.0;
	blue = 1.0;
	for(int i = 0; i < 4; i++)
		lastWhipDirection[i] = false;
		
	if(textureManager)
	{
		this->textureManager = textureManager;
		if(!this->textureManager->LoadTexture(textureName))
		{
			printf("Failed to load %s\n", textureName);
			return;
		}
		textureIndex = this->textureManager->TextureIndex(textureName);
	}
}
Beard::Beard(Vector2D * anchor, StringSegment * segments, int numSegments)
: Rope(anchor, segments, numSegments)
{
	red = 0.3176;
	green = 0.0980;
	blue = 0.0706;
	for(int i = 0; i < 4; i++)
		lastWhipDirection[i] = false;
}

Beard::Beard(Vector2D * anchor, int numSegments, float mass, float length, float springConstant)
: Rope(anchor, numSegments, mass, length, springConstant)
{
	red = 0.3176;
	green = 0.0980;
	blue = 0.0706;
	for(int i = 0; i < 4; i++)
		lastWhipDirection[i] = false;
}

Beard::Beard(Vector2D * anchor, int numSegments, float mass, float length, float springConstant,
			 float red, float green, float blue)
: Rope(anchor, numSegments, mass, length, springConstant)
{
	this->red = red;
	this->green = green;
	this->blue = blue;
	for(int i = 0; i < 4; i++)
		lastWhipDirection[i] = false;
}
Beard::~Beard()
{
	/*for(int i = 0; i < numSegments; i++)
	{
		delete(&segments[i]);
	}
	delete(segments);
	segments = NULL;
	numSegments = 0;
	delete(anchor);
	anchor = NULL;*/
}
void Beard::DrawQuad(Vector2D p1, Vector2D p2, float width1, float width2, float * startTexHeight)
{
	Vector2D dir = p2 - p1;
	float dist = dir.Magnitude();
	dir = !dir;
	dir = Vector2D(-dir.y, dir.x);
	
	Vector2D a = p1 - width1 * dir;
	Vector2D b = p1 + width1 * dir;
	Vector2D c = p2 - width2 * dir;
	Vector2D d = p2 + width2 * dir;
	

	
	glTexCoord2f(0,*startTexHeight);
	glVertex2f(b.x, b.y);
	glTexCoord2f(1,*startTexHeight);
	glVertex2f(a.x, a.y);
	*startTexHeight += dist / 10.0;
	
	glTexCoord2f(1,*startTexHeight);
	glVertex2f(c.x, c.y);
	
	glTexCoord2f(0,*startTexHeight);
	glVertex2f(d.x, d.y);
	
}
void Beard::Draw()
{
	float topWidth = 0.5;
	float bottomWidth = 0.1;
	float widthDelta = (topWidth - bottomWidth)/(numSegments);
	
	glColor3f(red,green,blue);
	glDisable(GL_TEXTURE_2D);
	glPushMatrix();
	glLoadIdentity();
	if(textureManager)
	{
		textureManager->EnableTexture(textureIndex);
		glEnable(GL_TEXTURE_2D);
		glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE );
		glAlphaFunc( GL_GREATER, 0.01 );
  		glEnable( GL_ALPHA_TEST );
	}
	glBegin(GL_QUADS);

	float startTexHeight = 0;
	DrawQuad(*anchor, segments[0].endPoint, topWidth, topWidth - widthDelta, &startTexHeight);
	topWidth -= widthDelta;
	
	for(int i = 1; i < numSegments; i++)
	{
		DrawQuad(segments[i-1].endPoint, segments[i].endPoint, topWidth, topWidth - widthDelta, &startTexHeight);
		topWidth -= widthDelta;
	}
	glEnd();
	glPopMatrix();
}
void Beard::Whip(bool left, bool right, bool up, bool down)
{
	
		
	bool whipDirection[4] = {left, right, up, down};
	int count = 0;
	Vector2D directions[4] = {Vector2D(-1,0), Vector2D(1,0), Vector2D(0,1), Vector2D(0,-1)};
	Vector2D forceDirection = Vector2D(0,0);
	
	for(int i = 0; i < 4; i++)
	{
		if(whipDirection[i] && !lastWhipDirection[i])
		{
			forceDirection = forceDirection + directions[i];
			count ++;
		}
		lastWhipDirection[i] = whipDirection[i];
		
	}
	
	if(count > 0)
	{
		forceDirection = forceDirection / count;
	
		if(grabbingObject)
		{
			
			//grabObject->AddForce(forceDirection * 2000, *grabPoint);
			//grabObject->AddForce(forceDirection * 200000);
			Release();
		}
		else
		{
			if(pauseUntilShrink)
				return;
			pauseUntilShrink = true;
		
			checkCollision = true;
			segments[numSegments - 1].AddForce(forceDirection * BEARD_WHIP_FORCE);	
		}
	}
}
void Beard::DirectionalWhip(Vector2D direction)
{
	if(!grabbingObject && !pauseUntilShrink)
	{
		pauseUntilShrink = true;
		checkCollision = true;
		direction = !direction;
		segments[numSegments - 1].AddForce(direction * BEARD_WHIP_FORCE);
	}
}
void Beard::Whip(Vector2D mousePoint)
{
	
	
	if(!grabbingObject && !pauseUntilShrink)
	{
		pauseUntilShrink = true;
		checkCollision = true;
		Vector2D direction = mousePoint - segments[numSegments - 1].endPoint;
		direction = !direction;
		segments[numSegments - 1].AddForce(direction * BEARD_WHIP_FORCE);
	}
}
void Beard::Move(Vector2D mousePoint)
{
	
	if(grabbingObject)
	{
		if(mousePoint.dist(*anchor) > 25)
		{
			mousePoint = *anchor + !(mousePoint - *anchor) * 25;
		}
		Vector2D direction = mousePoint - segments[numSegments - 1].endPoint;
		//direction = !direction;
		controllingObject = true;
		//float beardLength = GetLength();
		//if(direction.Magnitude() > (15.0 - beardLength) && beardLength < 15)
		//{
		//	direction = !direction * 15.0 - beardLength);
		//}
		
		//Vector2D force = ( direction / (grabObject->lastElapsedTime * grabObject->lastElapsedTime) - grabObject->velocity/(grabObject->lastElapsedTime)) * grabObject->mass;
		
		float dist = direction.Magnitude();
		//direction = !direction;
		
		
		float dirVel = grabObject->velocity.dot(direction);
		
		float k = 15000;
		if(grabObject->mass < 1.0 || grabObject->objectId == PINNEDOBJECT_CLASS)//|| grabObject->isColliding)
			k = 1500;
		//if( || grabObject->isColliding)
		//	k = 1000;
		Vector2D force = (direction * k - grabObject->velocity * 1.0 * sqrt(k * grabObject->mass));
		
		//if( beardLength > 25 && force.dot(segments[numSegments - 1].endPoint - *anchor) > 0)
		//	return;
		
		//if( force.Magnitude() < 0.1 )
		//	force = Vector2D(0,0);
		//	force = !force * GRAVITATIONAL_FORCE * -20.0;
		//if(magnitude > GRAVITATIONAL_FORCE * -10.0)
		//	magnitude = GRAVITATIONAL_FORCE * -10.0;
			
		//grabObject->AddForce(direction  * magnitude);//, *grabPoint);
		//Vector2D gravitationalForce = Vector2D(0.0,GRAVITATIONAL_FORCE) * grabObject->mass * -1.0;///(float)numVertices;
		//
		if(grabObject->objectId == PINNEDOBJECT_CLASS)
			grabObject->AddForceObjectSpace(force, *grabPoint);
		else
		{
			grabObject->AddForce(force);
			//grabObject->AddDamage(force.Magnitude()/100000.0);
		}
		//for(int i = numSegments/2; i < numSegments; i++)
			//segments[i].SetDampingConstant(grabObject->mass);
		//segments[numSegments-1].SetDampingConstant(grabObject->mass);
	}
}

